# cython: boundscheck=False
# cython. wraparound=False
# cython: cdivision=True
# cython: profile=False
# file: camori_aux.pyx


# Copyright (c) 2012   Universidade de São Paulo et alii. See AUTHORS
# file for details.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from __future__ import division
import numpy as np
cimport numpy as np
DTYPE = np.float32
ctypedef np.float32_t DTYPE_t
DTYPE2 = np.float64
ctypedef np.float64_t DTYPE2_t
CTYPE = np.uint8
ctypedef np.uint8_t CTYPE_t

cimport cython



cdef double project_point(double *out_x, double *out_y,
                          double x, double y, double z,
                          double* i_param):

    cdef double ux, uy, factor

    cdef double r2,r4,r6,r8,k1,k2, rho

    cdef int which_camera_model = int(i_param[0])

    if which_camera_model == 0 or which_camera_model == 1 or which_camera_model == 2:
        ux = i_param[1] * x/z
        uy = i_param[1] * y/z

    if which_camera_model == 0:
    ## Simple perspective projection.
        out_x[0] = i_param[2] + ux
        out_y[0] = i_param[3] + uy

    elif which_camera_model == 1:
    ## Polynomial
        factor = 1.0/sqrt(fabs(1 + 1e-9 * i_param[4] * (ux*ux+uy*uy)))
        out_x[0] = i_param[2] + ux * factor
        out_y[0] = i_param[3] + uy * factor

    elif which_camera_model == 2:
    ## Harris distortion model.
        factor = 1.0/sqrt(fabs(1 + 1e-9 * i_param[4] * (ux*ux+uy*uy)))
        out_x[0] = i_param[2] + ux * factor
        out_y[0] = i_param[3] + uy * factor

    elif which_camera_model == 3:
    ## Polar azimuhtal equidistant projection
        ux = x
        uy = y
        factor = hypot(ux,uy)
        ux = ux / factor
        uy = uy / factor
        rho = atan2(factor, z)
        out_x[0] = i_param[2] + i_param[1]*ux*rho
        out_y[0] = i_param[3] + i_param[1]*uy*rho





cdef double point_direction(double* out_x, double* out_y,
                            double* out_z, double x, double y,
                            double* i_param):
    
    cdef double ux, uy, factor
    cdef double rx, ry, rho, the, phi
    cdef double r2, r4, r6, r8, k1, k2

    cdef int which_camera_model = int(i_param[0])

    ## Simple perspective projection.
    if which_camera_model == 0:
        out_x[0] = x - i_param[2]
        out_y[0] = y - i_param[3]
        out_z[0] = i_param[1]

    ## Harris distortion model.
    elif which_camera_model == 2:
        ux = x - i_param[2]
        uy = y - i_param[3]
        factor = 1.0/sqrt(fabs(1 - 1e-9*i_param[4] * (ux*ux+uy*uy)))
        ux = ux * factor
        uy = uy * factor

        out_x[0] = ux
        out_y[0] = uy
        out_z[0] = i_param[1]

    ## Polar azimuhtal equidistant projection
    elif which_camera_model == 3:
        rx = (x - i_param[2])/i_param[1]
        ry = (y - i_param[3])/i_param[1]
        phi = sqrt(rx*rx+ry*ry)
        rx = rx/phi
        ry = ry/phi
        out_x[0] = sin(phi)*rx
        out_y[0] = sin(phi)*ry
        out_z[0] = cos(phi)

    ## Equiretangular
    elif which_camera_model == 4:
        if i_param[3] == 0:
            the = (x - i_param[1]) / i_param[0]
            phi = (y - i_param[2]) / i_param[0]

            out_x[0] = cos(phi) * sin(the)
            out_y[0] = sin(phi)
            out_z[0] = cos(phi) * cos(the)

    ## Cylindrical, with the z-axis as the zenith.
    elif which_camera_model == 5:
        the = (x - i_param[1]) / i_param[0]
        phi = (y - i_param[2]) / i_param[0]
        
        out_x[0] = sin(the)
        out_y[0] = cos(the)
        out_z[0] = -phi

    ## stereographic projection
    elif which_camera_model == 6:
        ux = (x - i_param[1]) / i_param[0]
        uy = (y - i_param[2]) / i_param[0]

        factor = 1.0/(1+ux*ux+uy*uy)
        out_x[0] = 2*ux*factor
        out_y[0] = 2*uy*factor
        out_z[0] = -(-1+ux*ux+uy*uy) * factor


def reproject(np.ndarray[CTYPE_t, ndim=3, mode="c"] out not None,
              np.ndarray[CTYPE_t, ndim=3, mode="c"] src not None,
              np.ndarray[DTYPE2_t, ndim=1, mode="c"] i_param_out not None,
              np.ndarray[DTYPE2_t, ndim=1, mode="c"] i_param_src not None,
              np.ndarray[DTYPE2_t, ndim=2, mode="c"] R_np not None
              ):

    cdef double qx,qy,qz
    cdef double rx,ry,rz

    cdef double sk, sj
    cdef int isk, isj

    assert R_np.shape[0] == 3
    assert R_np.shape[1] == 3
    cdef double* R = <double*> R_np.data

    cdef char* outd = <char*> out.data
    cdef char* srcd = <char*> src.data

    Ncol_out = out.shape[1]
    Ncol_src = src.shape[1]

    cdef double* ipo_data = <double*> i_param_out.data
    cdef double* ips_data = <double*> i_param_src.data

    for j in range(out.shape[0]):
        for k in range(out.shape[1]):
            point_direction(&qx, &qy, &qz, k, j, ipo_data)
            rx = R[0]*qx+R[1]*qy+R[2]*qz
            ry = R[3]*qx+R[4]*qy+R[5]*qz
            rz = R[6]*qx+R[7]*qy+R[8]*qz
            project_point(&sk, &sj, rx, ry, rz, ips_data)
            isk = lround(sk)
            isj = lround(sj)
            if (isj < 0 or isj >= src.shape[0] or
                isk < 0 or isk >= src.shape[1]):
                continue
            outd[j*Ncol_out*3 + k*3 + 0] = srcd[isj*Ncol_src*3 + isk*3  ]
            outd[j*Ncol_out*3 + k*3 + 1] = srcd[isj*Ncol_src*3 + isk*3 + 1]
            outd[j*Ncol_out*3 + k*3 + 2] = srcd[isj*Ncol_src*3 + isk*3 + 2]

def reprojection_limits(np.ndarray[CTYPE_t, ndim=3, mode="c"] out not None,
              np.ndarray[CTYPE_t, ndim=3, mode="c"] src not None,
              np.ndarray[DTYPE2_t, ndim=1, mode="c"] i_param_out not None,
              np.ndarray[DTYPE2_t, ndim=1, mode="c"] i_param_src not None,
              np.ndarray[DTYPE2_t, ndim=2, mode="c"] R_np not None
              ):
    ## Where the output vectors will be stored
    cdef np.ndarray lims = np.zeros([4,2], dtype=DTYPE2)
    cdef double qx,qy,qz
    cdef double rx,ry,rz

    cdef double* ipo_data = <double*> i_param_out.data
    cdef double* ips_data = <double*> i_param_src.data

    assert R_np.shape[0] == 3
    assert R_np.shape[1] == 3
    cdef double* R = <double*> R_np.data

    cdef double sk, sj
    
    point_direction(&qx, &qy, &qz, 0,0, ips_data)
    rx = R[0]*qx+R[3]*qy+R[6]*qz
    ry = R[1]*qx+R[4]*qy+R[7]*qz
    rz = R[2]*qx+R[5]*qy+R[8]*qz
    project_point(&sk, &sj, rx, ry, rz, ipo_data)
    lims[0,0] = sk
    lims[0,1] = sj
    
    point_direction(&qx, &qy, &qz, src.shape[1], 0, ips_data)
    rx = R[0]*qx+R[3]*qy+R[6]*qz
    ry = R[1]*qx+R[4]*qy+R[7]*qz
    rz = R[2]*qx+R[5]*qy+R[8]*qz
    project_point(&sk, &sj, rx, ry, rz, ipo_data)
    lims[1,0] = sk
    lims[1,1] = sj
    
    point_direction(&qx, &qy, &qz, 0,src.shape[0], ips_data)
    rx = R[0]*qx+R[3]*qy+R[6]*qz
    ry = R[1]*qx+R[4]*qy+R[7]*qz
    rz = R[2]*qx+R[5]*qy+R[8]*qz
    project_point(&sk, &sj, rx, ry, rz, ipo_data)
    lims[2,0] = sk
    lims[2,1] = sj
    
    point_direction(&qx, &qy, &qz, src.shape[1],src.shape[0], ips_data)
    rx = R[0]*qx+R[3]*qy+R[6]*qz
    ry = R[1]*qx+R[4]*qy+R[7]*qz
    rz = R[2]*qx+R[5]*qy+R[8]*qz
    project_point(&sk, &sj, rx, ry, rz, ipo_data)
    lims[3,0] = sk
    lims[3,1] = sj

    return lims





cdef extern double hypot(double,double)
cdef extern double sqrt(double)
cdef extern double fabs(double)
cdef extern long int lround(double)
cdef extern double tan(double)
cdef extern double atan2(double,double)
cdef extern double cos(double)
cdef extern double sin(double)
